
#ifndef PLANAR_MAPPING_H
#define PLANAR_MAPPING_H
#include <cv_bridge/CvBridge.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <robot_control/Landmarks.h>
#include <robot_control/Trajectory.h>
#include "ekf.h"

#include "common.h"
#include "graph_manager.h"
#include "occupancy_map.h"
#include "robot_interface.h"


class PlanarMapping {

 public:
  PlanarMapping();

  GraphManager* graph_mgr_;
  void planarCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
  void odometryCallback(double x, double y, double rot, double velocity,
			double rotation_speed, int priority);
  void landmarksCallback(const robot_control::LandmarksConstPtr& msg);
 
 protected:
	void pclToOccupancyMap_(const pcl::PointCloud<pcl::PointXYZRGB>& pcl_map,
			const tf::Transform& transform);
	void detectBlobs(void);
	void walkBlob(OccupancyMap *cells_tmp, int x, int y, int newId, Blob *new_blob);
	int computeTargetPath(const Coordinates &goal_target_pose,
			Trajectory *path);
	double getPointsDistance(const Coordinates &a, const Coordinates &b) const;
	double getPointsDeltaAngle(const Coordinates &a, 
			const Coordinates &b, double cur_angle) const;
			
	bool selectWaypoint(void);
	void StopRobot(void) const;
	bool pointIsInVisibleRange(int x, int y);
	void scanViewRange(bool clear=false);

  ros::NodeHandle nh_;
  ros::Publisher map_pub_;
  ros::Publisher camera_pose_pub_;
  ros::Publisher dummy_map_pub_;
  ros::Publisher initial_pose_pub_;
  ros::Subscriber planar_sub_;
  ros::Subscriber landmarks_sub_;
	
	RobotInterface robot_;

	double z_tolerance_;
  double z_tolerance_up_;
  double z_tolerance_down_;
  double z_offset_;
  
	Coordinates goal_target_;
	OccupancyMap occupancy_map_;
	tf::TransformListener tr_listener_; 
  FILE *camera_pose_file;
  Trajectory camera_trajectory_;
  Trajectory odometry_trajectory_;
  Trajectory landmarks_trajectory_;
	Targets targets_;

	Trajectory *goal_path_;
	const Coordinates *cur_waypoint_;

	// This is used only for drawing the targets on the occupancy map
  Trajectory targets_fake_trajectory_;
  Trajectory waypoint_fake_trajectory_;
  Trajectory goal_traj_;

	boost::thread *astar_worker_;
 
	bool use_shortest_path_;

	double curr_pose_x_;
	double curr_pose_y_;
	double curr_pose_rot_;

	double old_velocity_;
	double old_rotation_speed_;
	ros::Time last_ekf_time_;

	double curr_velocity_;
	double curr_rotation_speed_;

	bool random_rotation_done_;
	bool initial_path_;
	int robot_priority_;
	int camera_pose_id_;
	Ekf ekf_;
};

#endif

